#ifndef DH_GRIPPER_CTL_HPP
#define DH_GRIPPER_CTL_HPP

#include <string>
#include <vector>
#include <chrono>

#include <xarm_api/xarm_msgs.h>
#include "xarm_api/xarm_ros_client.h"

namespace hardware_ctl {

    std::vector<unsigned char> getGripperModbusCode(const std::vector<unsigned char>& data);
    std::pair<unsigned char, unsigned char> CRC16(const std::vector<unsigned char>& data);

    void init_gripper(std::shared_ptr<xarm_api::XArmROSClient> xarm_client);
    void open_gripper(std::shared_ptr<xarm_api::XArmROSClient> xarm_client);
    void close_gripper(std::shared_ptr<xarm_api::XArmROSClient> xarm_client);

    int16_t get_gripper_force(std::shared_ptr<xarm_api::XArmROSClient> xarm_client);
    void set_gripper_force(std::shared_ptr<xarm_api::XArmROSClient> xarm_client, int16_t force);
    
    int16_t get_gripper_position(std::shared_ptr<xarm_api::XArmROSClient> xarm_client);
    void set_gripper_position(std::shared_ptr<xarm_api::XArmROSClient> xarm_client, int16_t position);

}   // namespace hardware_ctl

#endif // DH_GRIPPER_CTL_HPP